
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_quat.c
  * @author     baiyang
  * @date       2021-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_math.h"
#include "gp_quat.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/** 
  * @brief       四元数初始化
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note       函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h) 
  */
void quat_init(Quat_t* q)
{
    q->q0 = 1.0f;
    q->q1 = 0.0f;
    q->q2 = 0.0f;
    q->q3 = 0.0f;
}

/** 
  * @brief       四元数取模
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
float quat_length(const Quat_t* q)
{
    return sqrtf(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);
}

/** 
  * @brief       四元数单位化
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_norm(Quat_t* q)
{
    const float len = quat_length(q);
    if(!math_flt_zero(len))
    {
        const float invlen = 1.0f/len;
        q->q0 *= invlen;
        q->q1 *= invlen;
        q->q2 *= invlen;
        q->q3 *= invlen;
    }
}

/** 
  * @brief       四元数共轭（模为1=取逆）
  * @param[in]   qout  
  * @param[in]   qin  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_inverse(Quat_t* qout, const Quat_t* qin)
{
    qout->q0 = qin->q0;
    qout->q1 = -qin->q1;
    qout->q2 = -qin->q2;
    qout->q3 = -qin->q3;
}

/** 
  * @brief       四元数共轭（模为1=取逆）
  * @param[in]   qout  
  * @param[in]   qin  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
Quat_t quat_inverse2(const Quat_t* qin)
{
    Quat_t qout;

    qout.q0 = qin->q0;
    qout.q1 = -qin->q1;
    qout.q2 = -qin->q2;
    qout.q3 = -qin->q3;

    return qout;
}

/** 
  * @brief       将四元数共轭
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_invert(Quat_t* q)
{
    q->q1 = -q->q1;
    q->q2 = -q->q2;
    q->q3 = -q->q3;
}

/** 
  * @brief       四元数乘法
  * @param[in]   q  
  * @param[in]   left  
  * @param[in]   right  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
Quat_t quat_mult(const Quat_t* left, const Quat_t* right)
{
    Quat_t q;

    q.w = left->w * right->w - left->x * right->x - left->y * right->y - left->z * right->z;
    q.x = left->x * right->w + left->w * right->x + left->y * right->z - left->z * right->y;
    q.y = left->y * right->w + left->w * right->y + left->z * right->x - left->x * right->z;
    q.z = left->z * right->w + left->w * right->z + left->x * right->y - left->y * right->x;

    return q;
}

/** 
  * @brief       四元数乘法
  * @param[in]   q  
  * @param[in]   left  
  * @param[in]   right  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
Quat_t quat_mult2(const Quat_t* left, const Quat_t* middle, const Quat_t* right)
{
    Quat_t q1;
    Quat_t q2;

    q1.w = left->w * middle->w - left->x * middle->x - left->y * middle->y - left->z * middle->z;
    q1.x = left->x * middle->w + left->w * middle->x + left->y * middle->z - left->z * middle->y;
    q1.y = left->y * middle->w + left->w * middle->y + left->z * middle->x - left->x * middle->z;
    q1.z = left->z * middle->w + left->w * middle->z + left->x * middle->y - left->y * middle->x;

    q2.w = q1.w * right->w - q1.x * right->x - q1.y * right->y - q1.z * right->z;
    q2.x = q1.x * right->w + q1.w * right->x + q1.y * right->z - q1.z * right->y;
    q2.y = q1.y * right->w + q1.w * right->y + q1.z * right->x - q1.x * right->z;
    q2.z = q1.z * right->w + q1.w * right->z + q1.x * right->y - q1.y * right->x;

    return q2;
}

/** 
  * @brief       四元数求和
  * @param[in]   q  
  * @param[in]   left  
  * @param[in]   right  
  * @param[out]  
  * @retval      
  * @note        函数根据四元数加法定义编写
  */
void quat_add(Quat_t* q, const Quat_t* left, const Quat_t* right)
{
    q->w = left->w + right->w;
    q->x = left->x + right->x;
    q->y = left->y + right->y;
    q->z = left->z + right->z;
}

/** 
  * @brief       欧拉角转欧拉角
  * @param[in]   q  
  * @param[in]   roll  
  * @param[in]   pitch  
  * @param[in]   yaw  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_from_euler(Quat_t* q, const float roll, const float pitch, const float yaw)
{
    const float cr2 = cosf(roll * 0.5f);
    const float cp2 = cosf(pitch * 0.5f);
    const float cy2 = cosf(yaw * 0.5f);
    const float sr2 = sinf(roll * 0.5f);
    const float sp2 = sinf(pitch * 0.5f);
    const float sy2 = sinf(yaw * 0.5f);
    
    q->q0 = cr2*cp2*cy2 + sr2*sp2*sy2;
    q->q1 = sr2*cp2*cy2 - cr2*sp2*sy2;
    q->q2 = cr2*sp2*cy2 + sr2*cp2*sy2;
    q->q3 = cr2*cp2*sy2 - sr2*sp2*cy2;
}

/** 
  * @brief       通过四元数得到roll
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
float quat_get_roll(const Quat_t* q)
{
    return (atan2f(2.0f * (q->q0*q->q1 + q->q2*q->q3), 1.0f - 2.0f * (q->q1*q->q1 + q->q2 * q->q2)));
}

/** 
  * @brief       四元数得到pitch
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
float quat_get_pitch(const Quat_t* q)
{
    return math_asinf(2.0f*(q->q0 * q->q2 - q->q3 * q->q1));
}

/** 
  * @brief       四元数得到yaw
  * @param[in]   q  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
float quat_get_yaw(const Quat_t* q)
{
    return atan2f(2.0f * (q->q0 * q->q3 + q->q1 * q->q2), 1.0f - 2.0f * (q->q2 * q->q2 + q->q3 * q->q3));
}

/** 
  * @brief       四元数转欧拉角
  * @param[in]   q  
  * @param[in]   euler  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_to_euler(const Quat_t* q, Euler_t* euler)
{
    euler->roll  = quat_get_roll(q);
    euler->pitch = quat_get_pitch(q);
    euler->yaw   = quat_get_yaw(q);
}

/** 
  * @brief       轴角转四元数
  * @param[in]   q  
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_from_axis_angle1(Quat_t* q, const Vector3f_t* v)
{
    //计算轴角的 角度
    const float theta = vec3_length(v);
    if(math_flt_zero(theta))
    {
        q->q0 = 1.0f;
        q->q1 = 0.0f;
        q->q2 = 0.0f;
        q->q3 = 0.0f;
        return;
    }

    Vector3f_t axis = {0};
    //旋转轴
    vec3_mult(&axis, v, 1.0f/theta);

    quat_from_axis_angle2(q, &axis, theta);
}

/** 
  * @brief       轴角转四元数 轴维单位向量
  * @param[in]   q  
  * @param[in]   axis  
  * @param[in]   theta  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_from_axis_angle2(Quat_t* q, const Vector3f_t* axis, const float theta)
{
    if(math_flt_zero(theta))
    {
        q->q0 = 1.0f;
        q->q1 = 0.0f;
        q->q2 = 0.0f;
        q->q3 = 0.0f;
        return;
    }
    const float st = sinf(theta*0.5f);

    q->q0 = cosf(theta*0.5f);
    q->q1 = axis->x * st;
    q->q2 = axis->y * st;
    q->q3 = axis->z * st;
}

/** 
  * @brief       四元数转轴角
  * @param[in]   q  
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_to_axis_angle(const Quat_t* q, Vector3f_t* v)
{
    //计算sin/2
    const float ls = sqrtf(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);

    vec3_set(v, q->q1, q->q2, q->q3);
    if(!math_flt_zero(ls))
    {
        vec3_mult(v, v, 1.0f/ls);

        //现在角度为-pi-pi
        float theta = math_wrap_PI(2.0f * atan2f(ls, q->q0));
        vec3_mult(v, v, theta);
    }
}

/** 
  * @brief       四元数旋转根据轴角
  * @param[in]   q  
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_rotate(Quat_t* q, const Vector3f_t* v)
{
    Quat_t q1;

    quat_from_axis_angle1(&q1, v);

    *q = quat_mult(q, &q1);
}

/** 
  * @brief       轴角快速转四元数
  * @param[in]   q  
  * @param[in]   v  轴角
  * @param[out]  
  * @retval      
  * @note        小角度情况（<10°）函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_from_axis_angle_fast1(Quat_t* q, const Vector3f_t* v)
{
    const float theta = vec3_length(v);
    if (math_flt_zero(theta))
    {
        q->q0 = 1.0f;
        q->q1 = 0.0f;
        q->q2 = 0.0f;
        q->q3 = 0.0f;
        return;
    }

    Vector3f_t axis = {0};

    //旋转轴
    vec3_mult(&axis, v, 1.0f / theta);

    quat_from_axis_angle_fast2(q, &axis, theta);
}

/** 
  * @brief       轴角快速转四元数
  * @param[in]   q  
  * @param[in]   axis   旋转轴
  * @param[in]   theta  旋转角
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_from_axis_angle_fast2(Quat_t* q, const Vector3f_t* axis, const float theta)
{
    const float t2 = theta/2.0f;
    const float sqt2 = t2 * t2;
    
    //sin(theta/2)泰勒展开o(x^5)
    const float st2  = t2 - sqt2 * t2 / 6.0f;
    //cos（theta/2）泰勒展开o(x^6)
    const float co2  = 1.0f - sqt2/2.0f + sqt2 * sqt2 /24.0f;
    
    q->q0 = co2;
    q->q1 = axis->x * st2;
    q->q2 = axis->y * st2;
    q->q3 = axis->z * st2;
}

/** 
  * @brief       四元数快速旋转（小角度）
  * @param[in]   q  
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        函数参考ardupilot的四元数库 /libraies/AP_Math/quaternion.c(h)
  */
void quat_rotate_fast(Quat_t* q, const Vector3f_t* v)
{
    const float theta = vec3_length(v);

    if(math_flt_zero(theta))
    {
        return;
    }

    Vector3f_t axis = {0};
    vec3_mult(&axis, v, 1.0f/theta);

    Quat_t q1;
    quat_from_axis_angle_fast2(&q1, &axis, theta);

    *q = quat_mult(q, &q1);
}

/**
  * @brief       从机体坐标系转移到导航坐标系，v_d = C * v_s
  * @param[in]   q  
  * @param[in]   v_d  
  * @param[in]   v_s  
  * @param[out]  
  * @retval      
  * @note        根据实际需求编写，旋转矩阵的功能
  */
void quat_vec_rotate(const Quat_t* q, Vector3f_t* v_d, const Vector3f_t* v_s)
{
    Vector3f_t v_d_tmp = *v_d;

    float x2  = q->x * 2.0f;
    float y2  = q->y * 2.0f;
    float z2  = q->z * 2.0f;
    float wx2 = q->w * x2;
    float wy2 = q->w * y2;
    float wz2 = q->w * z2;
    float xx2 = q->x * x2;
    float yy2 = q->y * y2;
    float zz2 = q->z * z2;
    float xy2 = q->x * y2;
    float yz2 = q->y * z2;
    float xz2 = q->z * x2;

    //
    v_d_tmp.vec3[0] = v_s->vec3[0]*(1.0f - yy2 - zz2) + v_s->vec3[1]*(xy2 - wz2)       + v_s->vec3[2]*(xz2 + wy2);
    v_d_tmp.vec3[1] = v_s->vec3[0]*(xy2 + wz2)        + v_s->vec3[1]*(1.0f- xx2 - zz2) + v_s->vec3[2]*(yz2 - wx2);
    v_d_tmp.vec3[2] = v_s->vec3[0]*(xz2 - wy2)        + v_s->vec3[1]*(yz2 + wx2)       + v_s->vec3[2]*(1.0f - xx2 - yy2);

    *v_d = v_d_tmp;
}

/**
  * @brief       从导航坐标系转移到机体坐标系，v_d = C^T * v_s
  * @param[in]   rotation  
  * @param[in]   from[3]  
  * @param[in]   to[3]  
  * @param[out]  
  * @retval      
  * @note        v_b = C^T * v_n
  */
void  quat_inv_vec_rotate(const Quat_t* q, Vector3f_t* v_d, const Vector3f_t* v_s)
{
    float x2  = q->x * 2.0f;
    float y2  = q->y * 2.0f;
    float z2  = q->z * 2.0f;
    float wx2 = q->w * x2;
    float wy2 = q->w * y2;
    float wz2 = q->w * z2;
    float xx2 = q->x * x2;
    float yy2 = q->y * y2;
    float zz2 = q->z * z2;
    float xy2 = q->x * y2;
    float yz2 = q->y * z2;
    float xz2 = q->z * x2;

    //
    v_d->vec3[0] = v_s->vec3[0]*(1.0f - yy2 - zz2) + v_s->vec3[1]*(xy2 + wz2)        + v_s->vec3[2]*(xz2 - wy2);
    v_d->vec3[1] = v_s->vec3[0]*(xy2 - wz2)        + v_s->vec3[1]*(1.0f - xx2 - zz2) + v_s->vec3[2]*(yz2 + wx2);
    v_d->vec3[2] = v_s->vec3[0]*(xz2 + wy2)        + v_s->vec3[1]*(yz2 - wx2)        + v_s->vec3[2]*(1.0f - xx2 - yy2);
}

/*------------------------------------test------------------------------------*/


